/*****************************************************************************/
/* 
 * FILENAME			: mediatek.c
 *
 * DESCRIPTION		: Implements Mediatek GPS device from diydrones.com
 *
 * PUBLIC FUNCTIONS : 
 *					  uint8_t handleGPS(char rxchar,GPRMC_STRUCT *gprmc, GPGGA_STRUCT *gpgga);
 *					  uint8_t decodeGPRMC(char *buf, GPRMC_STRUCT *gprmc);
 *					  uint8_t decodeGPGGA(char *buf, GPGGA_STRUCT *gpgga);
 *
 * NOTES 			: - 
 * 
 * AUTHOR			: Diederich Kroeske (dkroeske@d68.nl)
 *
 * VERSION INFO		: V1.0	dkroeske		18Aug2011	Intial release.
 */
 /*****************************************************************************/

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <usart.h>
#include <ctype.h>
#include <util/delay.h>
#include "mediatek.h"
#include "usart.h"
#include "led.h"

// Defines
#define GPSBUFSIZE 200

// Storage for the nmea sequences
char gpsbuf[GPSBUFSIZE];
char decbuf[GPSBUFSIZE];
unsigned char bufi = 0;
unsigned char pstate = 0;

// Local function prototypes
uint8_t char2int(char c);
uint16_t hex2int(char *asc);

/*****************************************************************************/
void initGPS(void)
/*
 * Description	: Inits the Mediatek GPS from diydrones. Switch GPS to NMEA mode
 *
 * input		: -
 * output		: -
 * 
 * Step 1: 	First UART is initialized to 38400 bps. This is the default transmit speed
 * 			of the mediatek device.
 * Step 2:	Switch the device to NMEA mode to output RMC and GGA messages.
 */
/*****************************************************************************/
{
	// Default Mediatek GPS speed
	init_uart(38400);
	
	_delay_ms(200);
	txstr("$PGCMD,16,1,1,1,1,1*6B\r\n");	// Set Mediatek to output NMEA
	_delay_ms(200);
}

/*****************************************************************************/
uint8_t handleGPS(char rxchar, GPRMC_STRUCT *gprmc, GPGGA_STRUCT *gpgga)
/*
 * Description	: Handles incomming chars (from GPS device) and tries to
 *				  decode received strings in RMC and GGA messages.
 *
 * input		: ref to gpsrmc and gpgga structs. These are filled when
 *				  strings are succesfully decoded in NMEA messages.
 * output		: Returns !0 when gprmc AND gpgga messages are available
 * 
 * (1) This function is based on character driven statemachine. Tweek to meet
 *     your application.
 * (2) ...
 */
/*****************************************************************************/
{
	uint8_t retval = 0;

	switch(pstate)
	{
		// Loop for '$'. Start new nmea sequence
		case 0:
			if( rxchar == '$' )
			{
				bufi = 0;
				pstate = 1;
			}
			break;
		
		// Store rx chars in buf till 'lf' is received.
		case 1: // Check for CR
			if( rxchar == 0x0D )
			{
				pstate = 2;
			}
			else
			{
				if( bufi < GPSBUFSIZE )
				{
					gpsbuf[bufi++] = rxchar;
				}
				else
				{
					pstate = 0;
				}
			}
			break;
			
		case 2: // Check for LF
			if( rxchar == 0x0A )
			{
				// Add '\0' to satisfy string manipulation functions
				gpsbuf[bufi] = '\0';
				
				// copy buffer (decoder kills buffer!)
				strcpy(decbuf, gpsbuf);
				
				// Try to decode RMC strings else GGA's
				retval = decodeGPRMC(decbuf, gprmc);
				if( !retval )
				{
					retval = decodeGPGGA(gpsbuf, gpgga);
				}
			}
			pstate = 0;
			break;
		
		default:
			pstate = 0;
			break;
	}
	return retval;
}


/*****************************************************************************/
uint8_t decodeGPRMC(char *buf, GPRMC_STRUCT *gprmc)
/*
 * Description	: Convert strings to RMC structs if possible
 *
 * input		: buf contains NMEA message (without trailing $)
 *				  ref to gprmc struct is filled with RMC parts.
 *
 * output		: !0 when succesfull.
 * 
 * (1) buf is destroyed after calling this routine! For speed sake.
 * (2) On Atmega8L this routine takes measured approx. 200us max.
 */
/*****************************************************************************/
{
	uint8_t done = 0;				// bail out flag
	char *token;					// 
	uint8_t state = 0;  			// internal state
	uint8_t retval = 0; 			// Assume fault
	uint16_t checksum = 0;
	uint16_t calculatedChecksum = 0;
	uint8_t index = 0;
		
	// First, calculate checksum
	while( buf[index] != '*' && index < 200 )
	{
		calculatedChecksum ^= buf[index];
		index++;
	}
	
	// Second, parse GPRMC string
	state = 0;
	while( !done  )
	{
		/*
		 * Do not use strtok, strtok skips empty fields !!
		 */
		token = strsep(&buf,",*");
		
		/* 
		 * convert into struct
		 */
		switch( state )
		{
			case 0:
				if( 0 != strcmp("GPRMC",token) )
				{
					done = 1;
				}
				break;
			
			case 1: // UTC time
				gprmc->utc.h = (token[0]-'0')*10+token[1]-'0';
				gprmc->utc.m = (token[2]-'0')*10+token[3]-'0';
				gprmc->utc.s = (token[4]-'0')*10+token[5]-'0';
				break;
			
			case 2: // Status
				if( 'A' == token[0] )
				{
					gprmc->status = 1;
				}
				else
				{
					gprmc->status = 0;
					done = 1;
				}
				break;				

			case 3: // Latitude
				strcpy(gprmc->latitude, token);
				break;

			case 4: // N/S indicator
				gprmc->NS = token[0];
				break;

			case 5: // Longitude
				strcpy(gprmc->longitude, token);
				break;
			
			case 6: // E/W indicator
				gprmc->EW = token[0];
				break;
			
			case 7: // groundspeed [knots]
				strcpy(gprmc->gs, token);
				break;
			
			case 8: // track [degree] / course
				strcpy(gprmc->course, token);
				break;
			
			case 9: // date
				gprmc->date.d = (token[0]-'0')*10+token[1]-'0';
				gprmc->date.m = (token[2]-'0')*10+token[3]-'0';
				gprmc->date.y = (token[4]-'0')*10+token[5]-'0';
				break;
			
			case 10: // Magnetic Variation Degrees. skip
				break;
			
			case 11: // Magnetic Variation E/W);. skip
				break;
			
			case 12: // Mode
				gprmc->mode = token[0];						
				break;
			
			case 13: // Checksum
				checksum = hex2int(token);
				if( checksum == calculatedChecksum )
				{
					//tx('r');
					retval = 1;
				}
				break;
			
			default:
				done = 1;
				break;
		}
		state++;
	}
	return retval;
}

/*****************************************************************************/
uint8_t decodeGPGGA(char *buf, GPGGA_STRUCT *gpgga)
/*
 * Description	: Convert strings to GGA structs if possible
 *
 * input		: buf contains NMEA message (without trailing $)
 *				  ref to gprmc struct is filled with GGA parts.
 *
 * output		: !0 when succesfull.
 * 
 * (1) buf is destroyed after calling this routine! For speed sake.
 * (2) On Atmega8L this routine takes measured approx. 200us max.
 */
/*****************************************************************************/
{
	uint8_t done = 0;				// bail out flag
	char *token;					// 
	uint8_t state = 0;  			// internal state
	uint8_t retval = 0; 			// Assume fault
	uint16_t checksum = 0;
	uint16_t calculatedChecksum = 0;
	uint8_t index = 0;
	
	// First, calculate checksum
	while( buf[index] != '*' && index < 200 )
	{
		calculatedChecksum ^= buf[index];
		index++;
	}
	
	// Second, parse GPGGA string
	state = 0;
	while( !done  )
	{
		/*
		 * Do not use strtok, strtok skips empty fields !!
		 */
		token = strsep(&buf,",*");
		
		/* 
		 * convert into struct
		 */
		switch( state )
		{
			case 0: // GGA protocol header
				if( 0 != strcmp("GPGGA",token) )
				{
					done = 1;
				}
				break;
			
			case 1: // UTC time
				gpgga->utc.h = (token[0]-'0')*10+token[1]-'0';
				gpgga->utc.m = (token[2]-'0')*10+token[3]-'0';
				gpgga->utc.s = (token[4]-'0')*10+token[5]-'0';
				break;			

			case 2: // Latitude
				strcpy(gpgga->latitude, token);
				break;

			case 3: // N/S indicator
				gpgga->NS = token[0];
				break;

			case 4: // Longitude
				strcpy(gpgga->longitude, token);
				break;
			
			case 5: // E/W indicator
				gpgga->EW = token[0];
				break;
			
			case 6: // Position Fix Indicator
				gpgga->pfi = token[0];
				break;
			
			case 7: // Satellites Used
				gpgga->satellitesUsed = token[0];
				break;

			case 8: // HDOP, Horizontal Dilution of Precision
				strcpy(gpgga->hdop,token);
				break;

			case 9: // MSL Altitude (mean sea level, calculated)
				strcpy(gpgga->mslAltitude, token);
				break;

			case 10: // units of antenne altitude
				break;

			case 11: // Geoidal Separation ?? skip
				break;

			case 12: // Units of geoidal seperation
				break;

			case 13: // Age of Diff. Corr. ?? Skip
				break;
			
			case 14: // This field is not in de mediatec datasheet !
				break;
			
			case 15: // Checksum
				checksum = hex2int(token);
				if( checksum == calculatedChecksum )
				{
					//tx('g');
					retval = 1;
				}
				break;
			
			default:
				done = 1;
				break;
		}
		state++;
	}

	return retval;
}


/*
 * 
 * Fast ascii to int
 *
 */
uint16_t hex2int(char *asc)
{		
	return( (char2int(asc[0]) << 4) + char2int(asc[1]));
}

uint8_t char2int(char c)
{
	uint8_t retval = 0;

	if( c >= '0' && c <= '9')
		retval = c - '0'; 
	else
		retval = toupper(c) + 10 - 'A';
	
	return retval;
}

